#include <rclcpp/rclcpp.hpp>
#include <mavros_msgs/msg/state.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <mavros_msgs/srv/set_mode.hpp>
#include <mavros_msgs/srv/command_bool.hpp>

using namespace std::chrono_literals;

class OffboardControl : public rclcpp::Node {
public:
    OffboardControl() : Node("offboard_control_node") {
        namespaces = "drone_0";
        state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
            namespaces +"/state", 10, std::bind(&OffboardControl::state_callback, this, std::placeholders::_1));

        local_pos_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
            namespaces +"/setpoint_position/local", 10);

        set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(namespaces +"/set_mode");
        arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>(namespaces +"/cmd/arming");

        // 使用 wall_timer 创建定时器，而不是重复 spin。
        timer_ = this->create_wall_timer(
            50ms, std::bind(&OffboardControl::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "Offboard control node initialized.");
        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
       namespaces +"/local_position/pose", 100, std::bind(&OffboardControl::pose_sub, this, std::placeholders::_1));
    }

private:
    void state_callback(const mavros_msgs::msg::State::SharedPtr msg) {
        current_state_ = *msg;
    }

    void timer_callback() {
        if (!current_state_.connected) {
            RCLCPP_WARN(this->get_logger(), "Waiting for FCU connection...");
            RCLCPP_INFO(this->get_logger(), "Please connect to your drone first.%d",current_state_.connected);
            return;
        }

        if (initial_setup_) {
            for (int i = 0; i < 100 && rclcpp::ok(); ++i) {
                local_pos_pub_->publish(target_pose_);
                rclcpp::sleep_for(50ms); // 在初始设置阶段发布位置
            }
            initial_setup_ = false;
            last_request_ = this->get_clock()->now();
        }

        // 切换到 OFFBOARD 模式
        if (current_state_.mode != "OFFBOARD" &&
            (this->get_clock()->now() - last_request_ > rclcpp::Duration(5.0))) {
            auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
            request->custom_mode = "OFFBOARD";

            if (set_mode_client_->wait_for_service(1s)) {
                auto result = set_mode_client_->async_send_request(request);
                if (result.wait_for(1s) == std::future_status::ready && result.get()->mode_sent) {
                    RCLCPP_INFO(this->get_logger(), "Offboard mode enabled.");
                }
            }
            last_request_ = this->get_clock()->now();
        }

        // 解锁无人机
        if (!current_state_.armed &&
            (this->get_clock()->now() - last_request_ > rclcpp::Duration(5.0))) {
            auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
            request->value = true;

            if (arming_client_->wait_for_service(1s)) {
                auto result = arming_client_->async_send_request(request);
                if (result.wait_for(1s) == std::future_status::ready && result.get()->success) {
                    RCLCPP_INFO(this->get_logger(), "Vehicle armed.");
                }
            }
            last_request_ = this->get_clock()->now();
        }

        // 发布目标位置
        local_pos_pub_->publish(target_pose_);
    }

    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr local_pos_pub_;
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    mavros_msgs::msg::State current_state_;
    rclcpp::Time last_request_;
    bool initial_setup_ = true;
    std::string namespaces;

    // 目标位置
    geometry_msgs::msg::PoseStamped target_pose_ = [] {
        geometry_msgs::msg::PoseStamped pose;
        pose.pose.position.x = 4.0;
        pose.pose.position.y =  4.0;
        pose.pose.position.z = 2; // 目标高度：10米
        return pose;
    }();

    void pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(),"receiev posisiton");
    geometry_msgs::msg::PoseStamped UAV_position = *msg;
    bool b_arrived = false;
    RCLCPP_INFO(this->get_logger(),"receiev posisiton:x= %f,y= %f,z = %f",
        UAV_position.pose.position.x,UAV_position.pose.position.y,UAV_position.pose.position.z
    );
    if(abs(UAV_position.pose.position.z - 10)<0.5){
        b_arrived = true;
    }else{
        b_arrived = false;
    }
}
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<OffboardControl>();

    // 确保只对 node 进行一次 spin 操作。
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
